#include "vectorn.h"

template <class T>
_MatrixMN<T>::_MatrixMN() : _row(0), _col(0), _buf(NULL)
{
}

template <class T>
_MatrixMN<T>::_MatrixMN(unsigned long row, unsigned long col) :_buf(NULL)
{
	SetSize(row, col);
	MakeZero();
}

template <class T>
_MatrixMN<T>::_MatrixMN(unsigned long row, unsigned long col, T* buf) :_buf(NULL)
{
	SetSize(row, col);
	memcpy(_buf, buf, row * col * sizeof(T));
}

template <class T>
_MatrixMN<T>::_MatrixMN(const _MatrixMN<T> &m) :_buf(NULL)
{
	SetSize(m._row, m._col);
	memcpy(_buf, m._buf, m._row * m._col * sizeof(T));
}

template <class T>
_MatrixMN<T>::~_MatrixMN()
{
	if(_buf != NULL)
	{
		delete []_buf;
		_buf = NULL;
	}
}

template <class T>
_MatrixMN<T>& _MatrixMN<T>::operator =(const _MatrixMN<T> &m)
{
	if (this == &m)
	{
		return *this;
	}

	if(_buf)
	{
		delete []_buf;
		_buf = NULL;
	}

	SetSize(m._row, m._col);
	memcpy(_buf, m._buf, m._row * m._col * sizeof(T));

	return *this;
}

template <class T>
T& _MatrixMN<T>::operator()(unsigned long row, unsigned long col)
{
	assert(row < _row && col < _col);
	return *(_buf + row * _col + col);
}

template <class T>
T _MatrixMN<T>::operator()(unsigned long row, unsigned long col) const
{
	assert(row < _row && col < _col);
	return *(_buf + row * _col + col);
}

template <class T>
unsigned long _MatrixMN<T>::GetRowCount() const
{
	return _row;
}

template <class T>
unsigned long _MatrixMN<T>::GetColCount() const
{
	return _col;
}

template <class T>
void _MatrixMN<T>::SetSize(unsigned long row, unsigned long col)
{
	_row = row;
	_col = col;

	if(_buf)
	{
		delete []_buf;
	}

	_buf = new T[_row * _col];
}

template <class T>
void _MatrixMN<T>::SetColumn(unsigned long col, const _VectorN<T> &v)
{
	assert(col < _col && v.GetSize() == _row);
	for(unsigned long i = 0; i < _row; ++i)
	{
		operator()(i, col) = v[i];
	}
}

template <class T>
void _MatrixMN<T>::SetRow(unsigned long row, const _VectorN<T> &v)
{
	assert(row < _row && v.GetSize() == _col);
	for(unsigned long i = 0; i < _col; ++i)
	{
		operator()(row, i) = v[i];
	}
}

template <class T>
void _MatrixMN<T>::SetDiagonalEntries(const T &v)
{
	for(unsigned long i = 0; i < _row; i++)
	{
		operator()(i, i) = v;
	}
}

template <class T>
void _MatrixMN<T>::SetDiagonalEntries(const _VectorN<T> &v)
{
	assert(v.GetSize() >= rows);
	for(unsigned long i = 0; i < _row; i++)
	{
		operator()(i, i) = v[i];
	}
}

template <class T>
void _MatrixMN<T>::AddToDiagonal(const T &v)
{
	for(unsigned long i = 0; i < _row; ++i)
	{
		operator()(i, i) += v;
	}
}

template <class T>
void _MatrixMN<T>::MakeZero()
{
	memset(_buf, 0, _row * _col * sizeof(T));
}

template <class T>
void _MatrixMN<T>::MakeIdentity()
{
	SetDiagonalEntries((T)1);
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::SubMatrix(unsigned long row1, unsigned long col1,
					   unsigned long row2, unsigned long col2) const
{
	assert(row1 <= row2 && col1 <= col2);
	unsigned long row = row2 - row1 + 1;
	unsigned long col = col2 - col1 + 1;
	_MatrixMN<T> m(row, col);
	for (unsigned long i = 0; i < row; ++i)
	{
		for (unsigned long j = 0; j < col; ++j)
		{
			m.operator()(i, j) = operator() (i + row1, j + col1);
		}
	}
	return m;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::Transpose() const
{
	_MatrixMN<T> m(_col, _row);
	for(unsigned long i = 0; i < _row; ++i)
	{
		for(unsigned long j = 0; j < _col; ++j)
		{
			m(j, i) = operator()(i, j);
		}
	}

	return m;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::Inverse() const
{
	//assert non singular
	assert(_col == _row);
	//using the LU decomposition algorithm
	//A*inv(A) = I -> LUinv(A) = I
	//L(U*inv(A)) = I , Y = U*inv(A)
	//LY = I -> we compute Y
	//and from Y we compute inv(A)

	_MatrixMN<T> L(_row, _col);
	_MatrixMN<T> U(_row, _col);
	_MatrixMN<T> inverse(_row, _col);
	_MatrixMN<T> I(_row, _col);
	I.MakeIdentity();
	LUDecomposition(L, U);

	for(unsigned long i = 0; i < _row; ++i)
	{
		_VectorN<T> y(_row);
		for(unsigned long j = 0; j < _row; ++j)
		{
			T sum = 0;
			for(unsigned long k = 0; k < j; ++k)
			{
				sum += L(j, k) * y[k];
			}

			y[j] = (I(j, i) - sum) / L(j, j);
		}

		for(long j = _row - 1; j >= 0; --j)
		{
			T sum = 0;
			for(unsigned long k = j + 1; k < _row; ++k)
			{
				sum += U(j, k) * inverse(k, i);
			}

			inverse(j, i) = (y[j] - sum) / U(j, j);
		}
	}

	return inverse;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::PseudoInverse() const
{
	_MatrixMN<T> U, V, UT;
	_VectorN<T> W;

	int m = _row, n = _col;

	U.SetSize(m, n);
	U.MakeZero();
	W.SetSize(n);
	W.MakeZero();
	V.SetSize(n, n);
	V.MakeZero();

	SVD(U, W, V);

	UT = U.Transpose();

	for(unsigned long i = 0; i < n; ++i)
	{
		if(W[i] > EPSILON)
		{
			W[i] = 1 / W[i];
		}
	}

	_MatrixMN<T> ps(n, m);

	for(unsigned long  i = 0; i < n; ++i)
	{
		for(unsigned long j = 0; j < m; ++j)
		{
			for(unsigned long k = 0; k < n; ++k)
			{
				ps(i, j) += V(i, k) * UT(k, j) * W[k];
			}
		}
	}

	return ps;
}

template <class T>
void _MatrixMN<T>::ForwardSubstitution(_VectorN<T> &x, const _VectorN<T> &b)
{
	x[0] = b[0];
	for(unsigned long i = 1; i < _row; ++i)
	{
		T sum = 0;
		for(unsigned long j = 0; j < i; ++j)
		{
			sum += x[j] * operator()(i, j);
		}

		x[i] = b[i] - sum;
	}
}

template <class T>
void _MatrixMN<T>::BackwardSubstitution(_VectorN<T> &x, const _VectorN<T> &b)
{
	x[_row - 1] = b[_row - 1] / operator()(_row - 1, _row - 1);

	for(long i = _row - 2; i >= 0; --i)
	{
		T sum = 0;
		for(unsigned long j = i; j < _row; ++j)
		{
			sum += x[j] * operator()(i, j);
		}

		x[i] = (b[i]  - sum) / operator()(i, i);
	}

}

template <class T>
void _MatrixMN<T>::SolveSystem(_VectorN<T> &x, const _VectorN<T> &b)
{
	_MatrixMN<T> L(_row, _col), U(_row, _col);
	LUDecomposition(L, U);
	_VectorN<T> y(_row);
	L.ForwardSubstitution(y, b);
	U.BackwardSubstitution(x, y);
}

template <class T>
void _MatrixMN<T>::LUDecomposition(_MatrixMN<T> &L, _MatrixMN<T> &U) const
{
	for(unsigned long i = 0; i < _row; i++)
	{
		U(0, i) = operator()(0, i);
		L(i, 0) = operator()(i, 0) / operator()(0, 0);
	}

	for(unsigned long i = 1; i < _row; i++)
	{
		for (unsigned long j = i; j < _col; j++)
		{
			T sum = 0;

			for(unsigned long k = 0; k < i; k++)
			{
				sum += L(i, k) * U(k, j);
			}

			U(i, j) = operator()(i, j) - sum; 
		}

		L(i, i) = 1.0;

		for(unsigned long j = i + 1; j < _row; j++)
		{
			T sum = 0;

			for(unsigned long k = 0; k < i; k++)
			{
				sum += L(j, k) * U(k, i);
			}

			L(j, i) = (operator()(i, j) - sum) / U(i, i);
		}
	}
}

/*
	For an m-by-n matrix A with m >= n, the singular value decomposition is
	an m-by-n orthogonal matrix U, an n-by-n diagonal matrix S(stored in a length n vectorn ), and
	an n-by-n orthogonal matrix V so that A = U*S*V'.

	The singular values, sigma[k] = S[k][k], are ordered so that
	sigma[0] >= sigma[1] >= ... >= sigma[n-1].
*/
template <class T>
void _MatrixMN<T>::SVD(_MatrixMN<T> &U, _VectorN<T> &S, _MatrixMN<T> &V) const
{				
	//assert(_row <= _col);
	int m = _row;
	int n = _col;
	int nu = std::min(m, n);

	_VectorN<T> e(n);
	_VectorN<T> work(m);
	_MatrixMN<T> A(*this);
	// boolean 
	int i=0, j=0, k=0;

	// Reduce A to bidiagonal form, storing the diagonal elements
	// in s and the super-diagonal elements in e.

	int nct = std::min(m-1, n);
	int nrt = std::max(0, std::min(n-2, m));
	for (k = 0; k < std::max(nct,nrt); k++)
	{
		if (k < nct) 
		{

			// Compute the transformation for the k-th column and
			// place the k-th diagonal in S[k].
			// Compute 2-norm of k-th column without under/overflow.
			S[k] = 0;
			for (i = k; i < m; i++)
			{
				S[k] = hypot(S[k],A(i,k));
			}
			if (S[k] != 0.0) 
			{
				if (A(k,k) < 0.0)
				{
					S[k] = -S[k];
				}
				for (i = k; i < m; i++) 
				{
					A(i,k) /= S[k];
				}
				A(k,k) += 1.0;
			}
			S[k] = -S[k];
		}
		for (j = k+1; j < n; j++) 
		{
			if ((k < nct) && (S[k] != 0.0))  
			{

				// Apply the transformation.

				T t(0.0);
				for (i = k; i < m; i++) 
				{
					t += A(i,k)*A(i,j);
				}
				t = -t/A(k,k);
				for (i = k; i < m; i++) 
				{
					A(i,j) += t*A(i,k);
				}
			}

			// Place the k-th row of A into e for the
			// subsequent calculation of the row transformation.

			e[j] = A(k,j);
		}
		if (k < nct) 
		{

			// Place the transformation in U for subsequent back
			// multiplication.

			for (i = k; i < m; i++) 
			{
				U(i,k) = A(i,k);
			}
		}
		if (k < nrt)
		{

			// Compute the k-th row transformation and place the
			// k-th super-diagonal in e[k].
			// Compute 2-norm without under/overflow.
			e[k] = 0;
			for (i = k+1; i < n; i++) 
			{
				e[k] = hypot(e[k],e[i]);
			}
			if (e[k] != 0.0) 
			{
				if (e[k+1] < 0.0) 
				{
					e[k] = -e[k];
				}
				for (i = k+1; i < n; i++) 
				{
					e[i] /= e[k];
				}
				e[k+1] += 1.0;
			}
			e[k] = -e[k];
			if ((k+1 < m) & (e[k] != 0.0)) 
			{

				// Apply the transformation.

				for (i = k+1; i < m; i++) 
				{
					work[i] = 0.0;
				}
				for (j = k+1; j < n; j++) 
				{
					for (i = k+1; i < m; i++) 
					{
						work[i] += e[j]*A(i,j);
					}
				}
				for (j = k+1; j < n; j++) 
				{
					T t(-e[j]/e[k+1]);
					for (i = k+1; i < m; i++) 
					{
						A(i,j) += t*work[i];
					}
				}
			}
			// Place the transformation in V for subsequent
			// back multiplication.

			for (i = k+1; i < n; i++) 
			{
				V(i,k) = e[i];
			}
		}
	}

	// Set up the final bidiagonal matrix or order p.

	int p = std::min(n, m+1);
	if (nct < n) 
	{
		S[nct] = A(nct,nct);
	}
	if (m < p) 
	{
		S[p-1] = 0.0;
	}
	if (nrt+1 < p) 
	{
		e[nrt] = A(nrt,p-1);
	}
	e[p-1] = 0.0;

	// If required, generate U.

	for (j = nct; j < nu; j++) 
	{
		for (i = 0; i < m; i++) 
		{
			U(i,j) = 0.0;
		}
		U(j,j) = 1.0;
	}
	for (k = nct-1; k >= 0; k--) 
	{
		if (S[k] != 0.0) 
		{
			for (j = k+1; j < nu; j++) 
			{
				T t(0.0);
				for (i = k; i < m; i++) 
				{
					t += U(i,k)*U(i,j);
				}
				t = -t/U(k,k);
				for (i = k; i < m; i++) 
				{
					U(i,j) += t*U(i,k);
				}
			}
			for (i = k; i < m; i++ ) 
			{
				U(i,k) = -U(i,k);
			}
			U(k,k) = 1.0 + U(k,k);
			for (i = 0; i < k-1; i++) 
			{
				U(i,k) = 0.0;
			}
		} 
		else 
		{
			for (i = 0; i < m; i++) 
			{
				U(i,k) = 0.0;
			}
			U(k,k) = 1.0;
		}
	}

	// If required, generate V.

	for (k = n-1; k >= 0; k--) 
	{
		if ((k < nrt) & (e[k] != 0.0)) 
		{
			for (j = k+1; j < nu; j++) 
			{
				T t(0.0);
				for (i = k+1; i < n; i++)
				{
					t += V(i,k)*V(i,j);
				}
				t = -t/V(k+1,k);
				for (i = k+1; i < n; i++) 
				{
					V(i,j) += t*V(i,k);
				}
			}
		}
		for (i = 0; i < n; i++) 
		{
			V(i,k) = 0.0;
		}
		V(k,k) = 1.0;
	}

	// Main iteration loop for the singular values.

	int pp = p-1;
	T eps(pow(2.0,-52.0));
	while (p > 0) 
	{
		int k=0;
		int kase=0;

		// Here is where a test for too many iterations would go.

		// This section of the program inspects for
		// negligible elements in the s and e arrays.  On
		// completion the variables kase and k are set as follows.

		// kase = 1     if s(p) and e[k-1] are negligible and k<p
		// kase = 2     if s(k) is negligible and k<p
		// kase = 3     if e[k-1] is negligible, k<p, and
		//              s(k), ..., s(p) are not negligible (qr step).
		// kase = 4     if e(p-1) is negligible (convergence).

		for (k = p-2; k >= -1; k--) 
		{
			if (k == -1) 
			{
				break;
			}
			if (abs(e[k]) <= eps*(abs(S[k]) + abs(S[k+1]))) 
			{
				e[k] = 0.0;
				break;
			}
		}

		if (k == p-2) 
		{
			kase = 4;
		} 
		else 
		{
			int ks;
			for (ks = p-1; ks >= k; ks--) 
			{
				if (ks == k) 
				{
					break;
				}
				T t( (ks != p ? abs(e[ks]) : 0.) + 
					(ks != k+1 ? abs(e[ks-1]) : 0.));
				if (abs(S[ks]) <= eps*t)  
				{
					S[ks] = 0.0;
					break;
				}
			}

			if (ks == k) 
			{
				kase = 3;
			} 
			else 
				if (ks == p-1) 
				{
					kase = 1;
				} 
				else 
				{
					kase = 2;
					k = ks;
				}
		}
		k++;

		// Perform the task indicated by kase.

	switch (kase) 
	{
		// Deflate negligible s(p).
		case 1: 
			{
				T f(e[p-2]);
				e[p-2] = 0.0;
				for (j = p-2; j >= k; j--) 
				{
					T t( hypot(S[j],f));
					T cs(S[j]/t);
					T sn(f/t);
					S[j] = t;
					if (j != k) 
					{
						f = -sn*e[j-1];
						e[j-1] = cs*e[j-1];
					}

					for (i = 0; i < n; i++) 
					{
						t = cs*V(i,j) + sn*V(i,p-1);
						V(i,p-1) = -sn*V(i,j) + cs*V(i,p-1);
						V(i,j) = t;
					}

				}
			}
			break;

				// Split at negligible s(k).

		case 2: 
			{
				T f(e[k-1]);
				e[k-1] = 0.0;
				for (j = k; j < p; j++) 
				{
					T t(hypot(S[j],f));
					T cs( S[j]/t);
					T sn(f/t);
					S[j] = t;
					f = -sn*e[j];
					e[j] = cs*e[j];
					for (i = 0; i < m; i++) 
					{
						t = cs*U(i,j) + sn*U(i,k-1);
						U(i,k-1) = -sn*U(i,j) + cs*U(i,k-1);
						U(i,j) = t;
					}
				}
			}
			break;

				// Perform one qr step.

		case 3: 
			{
				// Calculate the shift.

				T scale = std::max(std::max(std::max(std::max(
					abs(S[p-1]),abs(S[p-2])),abs(e[p-2])), 
					abs(S[k])),abs(e[k]));
				T sp = S[p-1]/scale;
				T spm1 = S[p-2]/scale;
				T epm1 = e[p-2]/scale;
				T sk = S[k]/scale;
				T ek = e[k]/scale;
				T b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
				T c = (sp*epm1)*(sp*epm1);
				T shift = 0.0;
				if ((b != 0.0) || (c != 0.0)) 
				{
					shift = sqrt(b*b + c);
					if (b < 0.0) 
					{
						shift = -shift;
					}
					shift = c/(b + shift);
				}
				T f = (sk + sp)*(sk - sp) + shift;
				T g = sk*ek;

				// Chase zeros.

				for (j = k; j < p-1; j++) 
				{
					T t = hypot(f,g);
					T cs = f/t;
					T sn = g/t;
					if (j != k) 
					{
						e[j-1] = t;
					}
					f = cs*S[j] + sn*e[j];
					e[j] = cs*e[j] - sn*S[j];
					g = sn*S[j+1];
					S[j+1] = cs*S[j+1];
					for (i = 0; i < n; i++) 
					{
						t = cs*V(i,j) + sn*V(i,j+1);
						V(i,j+1) = -sn*V(i,j) + cs*V(i,j+1);
						V(i,j) = t;
					}
					t = hypot(f,g);
					cs = f/t;
					sn = g/t;
					S[j] = t;
					f = cs*e[j] + sn*S[j+1];
					S[j+1] = -sn*e[j] + cs*S[j+1];
					g = sn*e[j+1];
					e[j+1] = cs*e[j+1];
					if (j < m-1) 
					{
						for (i = 0; i < m; i++) 
						{
							t = cs*U(i,j) + sn*U(i,j+1);
							U(i,j+1) = -sn*U(i,j) + cs*U(i,j+1);
							U(i,j) = t;
						}
					}
				}
				e[p-2] = f;
			}
			break;

				// Convergence.

		case 4: 
			{
				// Make the singular values positive.

				if (S[k] <= 0.0) 
				{
					S[k] = (S[k] < 0.0 ? -S[k] : 0.0);
					for (i = 0; i <= pp; i++) 
					{
						V(i,k) = -V(i,k);
					}
				}

				// Order the singular values.

				while (k < pp) 
				{
					if (S[k] >= S[k+1]) 
					{
						break;
					}
					T t = S[k];
					S[k] = S[k+1];
					S[k+1] = t;
					if (k < n-1) 
					{
						for (i = 0; i < n; i++) 
						{
							t = V(i,k+1); V(i,k+1) = V(i,k); V(i,k) = t;
						}
					}
					if (k < m-1) 
					{
						for (i = 0; i < m; i++) 
						{
							t = U(i,k+1); U(i,k+1) = U(i,k); U(i,k) = t;
						}
					}
					k++;
				}
				p--;

			}
			break;
		}
	}
}

template <class T>
_VectorN<T> _MatrixMN<T>::operator*(const _VectorN<T> &v) 
{
	assert(v.GetSize() == _col);
	_VectorN<T> r(_row);
	for(unsigned long i = 0; i < _row; ++i)
	{
		for(unsigned long j = 0; j < _col; ++j)
		{
			r[i] += operator()(i,j) * v[j];
		}
	}

	return r;
}

template <class T>
_VectorN<T> operator*(const _VectorN<T> &v, const _MatrixMN<T> &m) 
{
	assert(v.GetSize() == m.row);
	_VectorN<T> r(m.col);
	for(unsigned long i = 0; i < m._col; i++ )
	{
		for(unsigned long j = 0; j < m._row; j++)
		{
			r[i] += m(j, i) * v[j];
		}
	}

	return r;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::operator*(const _MatrixMN<T> &r)
{
	assert(_col == r._row);
	_MatrixMN<T> result(_row, r._col);

	for(unsigned long i = 0; i < _row; i++)
	{
		for(unsigned long j = 0; j < r._col; j++)
		{
			for(unsigned long  k = 0; k < _col; k++)
			{
				result(i, j) += operator()(i, k) * r(k, j);
			}		
		}
	}

	return result;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::operator*(const T &scale)
{
	_MatrixMN<T> result(_row, _col);

	for(unsigned long i = 0; i < _row; i++)
	{
		for(unsigned long j = 0; j < _col; j++)
		{
			result(i, j) *= scale;
		}
	}

	return result;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::operator+(const _MatrixMN<T> &other)
{
	assert(_row == other._row && _col == other._col);
	_MatrixMN<T> result(_row, _col);

	for(unsigned long i = 0; i < _row; i++)
	{
		for(unsigned long j = 0; j < _col; j++)
		{
			result(i, j) = operator()(i, j) + other(i, j);
		}
	}

	return result;
}

template <class T>
_MatrixMN<T> _MatrixMN<T>::operator-(const _MatrixMN<T> &other)
{
	assert(_row == other._row && _col == other._col);
	_MatrixMN<T> result(_row, _col);

	for(unsigned long i = 0; i < _row; i++)
	{
		for(unsigned long j = 0; j < _col; j++)
		{
			result(i, j) = operator()(i, j) - other(i, j);
		}
	}

	return result;
}

